#include "Hchassis.h"

void Hchassis::fast_forward_enc_absolute(double rotation, double speed,
                                        int timeout_ms, Block waitForCompletion,
                                        rotationUnits units_r,
                                        velocityUnits units_v) {
  p_au_l->act_for_enc_absolute(rotation, speed, timeout_ms, Block::no, false, 0,
                               units_r, units_v);
  p_au_r->act_for_enc_absolute(rotation, speed, timeout_ms, waitForCompletion,
                               false, 0, units_r, units_v);
  wait(1);
}

void Hchassis::fast_forward_enc_relative(double rotation, double speed,
                                        int timeout_ms, Block waitForCompletion,
                                        rotationUnits units_r,
                                        velocityUnits units_v) {
  p_au_l->act_for_enc_relative(rotation, speed, timeout_ms, Block::no, false, 0,
                               units_r, units_v);
  p_au_r->act_for_enc_relative(rotation, speed, timeout_ms, waitForCompletion,
                               false, 0, units_r, units_v);
  wait(1);
}
//////////////////////////////////////////////////////////////////////////////////
void Hchassis::fast_forward_dist_absolute(double dist, double speed,
                                         int timeout_ms,
                                         Block waitForCompletion,
                                         distanceUnits units_d,
                                         velocityUnits units_v) {
  if (timeout_ms < 0) {
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }

  double shaft_aim_rev = distance_to_rev(dist, units_d);
  fast_forward_enc_absolute(shaft_aim_rev, speed, timeout_ms, waitForCompletion,
                            rotationUnits::rev, units_v);
  wait(1);
}

void Hchassis::fast_forward_dist_relative(double dist, double speed,
                                         int timeout_ms,
                                         Block waitForCompletion,
                                         distanceUnits units_d,
                                         velocityUnits units_v) {
  if (timeout_ms < 0) {
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }
  double shaft_aim_rev = distance_to_rev(dist, units_d);
  fast_forward_enc_relative(shaft_aim_rev, speed, timeout_ms, waitForCompletion,
                            rotationUnits::rev, units_v);
  wait(1);
}